% initalposition.m

function theta_init = initalposition()
    load("ConstantDefinition.mat", "theta0", "l0", "l1", "l2", "l3", "l4" ,"rM","Ly");

    rc_init = [rM(1), rM(2)+Ly/2];

    init_theta = [pi/2, 0, pi/4, pi];

    opt=optimset('Display','off');    % 关闭输出
    fun = @(x)Five_Link_Inv([l0, l1, l2, l3, l4], [theta0, x(1), x(2), x(3), x(4)], rc_init);
    [y,~,exitflag,output,~] = fsolve(fun, init_theta, opt);
    % if exitflag <= 0
    %     disp('Fsolve failed. Try more times!');
    % 
    %     while exitflag <= 0
    %         for i = 0 : 0.1*pi : pi
    %             for j = 0 : 0.1*pi : pi
    %                 for k = 0 : 0.1*pi : pi
    %                     for m = 0 : 0.1*pi : pi
    %                         [y,~,exitflag,output,~] = fsolve(fun, [i,j,k,m], opt);
    %                     end
    %                 end
    %             end
    %         end
    %     end
    % end

    theta_init = y;
end

function res = Five_Link_Inv(l, theta, rc)
    l0 = l(1);
    l1 = l(2);
    l2 = l(3);
    l3 = l(4);
    l4 = l(5);
    theta0 = theta(1);
    theta1 = theta(2);
    theta2 = theta(3);
    theta3 = theta(4);
    theta4 = theta(5);
    xc = rc(1);
    yc = rc(2);
    res(1) = l1*cos(theta1) + l2*cos(theta2) - xc;
    res(2) = l1*sin(theta1) + l2*sin(theta2) - yc;
    res(3) = l0*cos(theta0) + l3*cos(theta3) + l4*cos(theta4) - xc;
    res(4) = l0*sin(theta0) + l3*sin(theta3) + l4*sin(theta4) - yc;
end